﻿#include "StdAfx.h"
#include "utility.h"
#include "MainFunctions.h"
#include "GeneticAlgorithm.h"
#include "Counter.h"
#include "Globals.h"

enum TestingThreadState
{
	ready, evaluate, quit
} ThreadState;

DWORD	TestingThreadSleep = 100;

Paths *paths = NULL;

bool StopOptimizing = false;

//-----------------------------------------------------------------------------
//	worker thread 1 of 1
DWORD WINAPI TestingThread(LPVOID lpParam)
{
	SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_BELOW_NORMAL);
	while(true)
	{
		switch(ThreadState)
		{
			case evaluate:
			{
			//----
			//	test half the population
				for(int j = 0; paths && j < paths->getPopulationSize()/2; j++)
				{
					float fitnessBias = paths->arrangeDistance(j);
					paths->evaluate(j, fitnessBias);
					if(ThreadState==quit)
						return 0;
				}

				ThreadState = ready;
			}
			break;

			case quit:
				return 0;

			default:
				Sleep(TestingThreadSleep);
		}
	}

	return 0; 
}


//-----------------------------------------------------------------------------
void ExitTestingThread()
{
	ThreadState = quit;
}


const	float	R	= 2.0f,											// initialization range
				W	= 512.0f,										// fixed width  of (osus) coordinate system
				H	= 384.0f,										// fixed height of (osus) coordinate system
				TIME_TO_DISTANCE = 128.0f;						// pixel in (512*384) per second, for spacing of buttons

//-----------------------------------------------------------------------------
Paths::Paths(int num, int pop)
: CoSyNE(num*(sizeof(Path)/sizeof(float)), 0, pop), N(num), distances(NULL)
{

}

//-----------------------------------------------------------------------------
//	Sample initialization and mutation takes care to put the
//	first point outside the (512*384) screen space, and the last (4th)
//	point inside.

void	Paths::randomizeSample(int j)
{
//----
//	initialize strategy parameters with the generator variables
	for(int i = 0; i < StrategyWeights; i++)
		cache.getCol(j)[i] = generatorVariables[i];

//----
//	mutation of strategy parameters
	sampleMutateStrategyWeights(j,true);

	Path *paths = getPaths(j);

//----
//	Random initialize points.
//	We start with the points centered around (0,0), and in the end we move them by (w/2,h/2) into the center of the screen.
	Point bias(0.5f*W, 0.5f*H);
	for(int i = 0; i < N; i++)
	{
		paths[i].end = Point::FromAngle(float(rng.rand()*2.0*PI));
		paths[i].end.x *= (rng.randf()*0.5f*W);
		paths[i].end.y *= (rng.randf()*0.5f*H);
		paths[i].end += bias;
	}

//----
//	arrange endpoints by distance
	arrangeDistance(j);

	for(int i = 0; i < N; i++)
	{
		Path &p = paths[i];

	//	make sure the endpoint is not hit from the very edge of the screen.
		Point p2 = Point::FromAngle(float(rng.rand()*2.0*PI));
		p2.x *= (rng.randf()*0.2f*W);
		p2.y *= (rng.randf()*0.2f*H);
		Point end = p.end - bias,
				t   = end - p2;				//	impact vector
		t.normalize() *= (50.0f + 0.5f*W*rng.randf());
		p.dir = (end - t).normalize();	//	impact direction

	//----
	//	amplitude and angular velocity of the two sine functions
		M128 gauss;
		gauss.m128d = rng.gauss_pd();
		p.alpha1 = 192.0f / (1.0f+sqr(float(gauss.f64[0])));
		p.omega1 = 0.009f / (1.0f+sqr(float(gauss.f64[1]))) * (rng.rand() < 0.5 ? -1.0f : 1.0f);

		if(rng.rand() < 0.5)
		{
			gauss.m128d = rng.gauss_pd();
			p.alpha2 = 28.0f / (1.0f+sqr(float(gauss.f64[0])));
			p.omega2 = 0.01f / (0.6f+sqr(float(gauss.f64[1]))) * (rng.rand() < 0.5 ? -1.0f : 1.0f);
		}
		else
		{
			p.alpha2 = 0;
			p.omega2 = 0;
		}
	}
}

//-----------------------------------------------------------------------------
void	Paths::sampleMutateWeights(int j)
{
//----
//	possible mutation of each weight
	double	prob  = cache(MutateProb,j);
	__m128	gamma = _mm_set1_ps(cache(MutateGamma,j)),
				alpha = _mm_set1_ps(cache(MutateAlpha,j));

//----
//	Lèvy random numbers are expensive, so always make 4 at once.
	M128		levys;
	int		levysLeft = 0;

	Path		*paths = getPaths(j);

//----
//	mutate points
	for(int i = 0; i < N; i++)
	{
		float *p = (float*)&paths[i];		// 8 floats
		bool	normalize = false;			// renormalize dir if it's mutated

		for(int k = 0; k < 4; k++)			// only mutate the first 4 ('end' and 'dir')
		{
			if(rng.rand() < prob)
			{
				if(levysLeft==0)
				{
					levys.m128 = rng.levy(gamma,alpha);
					levysLeft = 4;
				}

				float f = p[k] + levys.f32[--levysLeft];

			//----
			//	the endpoint must lie inside the screen
				if			(k==0)			f = min(W, max(0.0f, f));
				else if	(k==1)			f = min(H, max(0.0f, f));
				if			(k==2 || k==3)	normalize = true;

				p[k] = f;
			}
		}

		if(normalize)
			paths[i].dir.normalize();
	}
}

//-----------------------------------------------------------------------------
//	d contains relative distances between all end points; d[0] is ignored.
float	Paths::arrangeDistance(int j)
{
	Path *paths = getPaths(j);

	float result = 0;

//----
//	set distances between the end points
	Point pPrev = paths[0].end;

	for(int i = 1; i < N; i++)
	{
		Point &p = paths[i].end;

		Point diff = p - pPrev;

		float l = diff.norm();
		if(l==0 && distances[i]!=0)
		{
		//	vector with the correct length and random direction
			diff = Point::FromAngle(float(2.0*PI*rng.rand())) * distances[i];
		}
		else
		{
		//	vector with the correct length in the same direction
			diff *= distances[i] / l;
		}

	// set new position with the correct distance from the previous point
		Point pNew = pPrev + diff;

	//----
	//	if p is not inside [(0,0),(W,H)], either collapse the whole combo into one point or keep the old point (which had the wrong distance) if this is a new combo.
		if(pNew.x >= W)	result += pNew.x - W;
		if(pNew.x <= 0)	result -= pNew.x;
		if(pNew.y >= H)	result += pNew.y - H;
		if(pNew.y <= 0)	result -= pNew.y;
		if(pNew.x <= W && pNew.x >= 0
		&&	pNew.y <= H && pNew.y >= 0)
		{
			p = pNew;
		}
		else if(i&0x1)							// the odd indices are the paths of the last button of a combo
		{
			p = paths[i-1].end;
		}

		pPrev = p;
	}

	return 0.01f*result;
}

#include <math.h>

//-----------------------------------------------------------------------------
//	The fitness function is an error function: lower means better.
//	How to combine the different errors?
//	At the moment all components are added together, which makes it possible to weight them.
//-----------------------------------------------------------------------------

void Paths::evaluate(int j, float bias)
{
	Path *paths = getPaths(j);
	const Point center(0.5f*W, 0.5f*H);

//	error components
	float e[6] = {0,0,0,0,0,0};

//----
	Point t2Prev	(0.0f,0.0f),											// prev endpoint change
			t2Combo	(0.0f,0.0f),											// direction of current combo
			dirPrev	(0.0f,0.0f),											// prev approach vector
			hitPrev	(0.0f,0.0f);											// prev impact vector

	for(int n = 0; n < N; n++)
	{
		float ex = (paths[n].end.x - center.x) - 0.8f*W,
				ey = (paths[n].end.y - center.y) - 0.8f*H;
		if(ex > 0) e[0] += 0.01f*ex;
		if(ey > 0) e[0] += 0.01f*ey;

		const Point &end	= paths[n].end,								// endpoint of the path
						&dir	= paths[n].dir;								// linear component of impact direction

	//----
	//	Get a point just before impact, to calculate impact direction
		float	s_		= 1024.0f*0.001f,
				phi1	= paths[n].omega1 * s_,
				sin1	= paths[n].alpha1 * sin(phi1);

		Point p1		= end - dir*s_ + dir.perp() * sin1,// + (t1.perp()+t1) * sin2,
				hit	= (end - p1).normalize();

		float omegaEnd	= (end - center).angle(),						// angle of the endpoint relative to the center of the screen
				omegaDir	= dir.angle(),										// approach angle
				omegaHit	= hit.angle();										// impact angle

	//----
	//	We should approach the endpoints from the center of the screen
		float squaredDistanceFromCenter = (end - center).normSquared();
		if(squaredDistanceFromCenter > 3600.0f)
		{
			e[1] += 0.00002f*squaredDistanceFromCenter*sqr(omegaEnd-omegaDir);
			e[1] += 0.00001f*squaredDistanceFromCenter*sqr(omegaEnd-omegaHit);
		}

		float	norm2 = 0.0f;

		if((n & 0x1) == 0)													// the even indices are the paths of the first button of a combo
		{
			t2Combo = paths[n+1].end - end;								// direction of current combo

			norm2 = t2Combo.norm();

			if(norm2 > 0.01f)	t2Combo *= (1.0f / norm2);
			else					t2Combo = t2Prev;
		}

		float	cosImpact = hit ^ t2Combo;
		if(cosImpact < 0)
			cosImpact = sqr(cosImpact) - cosImpact;					// impacts opposite to the current direction are very bad, orthogonal are best.
		e[2] += sqr(2.0f * cosImpact);

		if(n >= 1)
		{
			Point t2;

			if(n & 0x1)
				t2 = t2Combo;
			else
			{
				t2 = end - paths[n-1].end;									// direction between endpoints of current combo and between combos

				norm2 = t2.norm();

				if(norm2 > 0.01f)	t2 *= (1.0f / norm2);
				else					t2 = t2Prev;
			}

		//----
		//	check distances to previous points, this is similar to e[2]
			if(n & 0x1)
			{
				int lim = min(n,10);
				float minDistance = distances[n];
				Point	p0 = (end - paths[n-1].end)*0.5f;				// the midpoint of the current combo
				for(int k = 2; k <= lim; k+=2)
				{
					minDistance += distances[n-k];
					minDistance += distances[n-k+1];
					Point	p1 = (paths[n-k].end - paths[n-k-1].end)*0.5f,
							t = p0 - p1;										// the vector from the midpoint of a previous combo
					e[5] += 0.01f * minDistance / (1.0f+t.norm());	// get away from the old points
				}
			}

			if(n >= 2)
			{
				float cosChange = t2 ^ t2Prev;
				e[3] += 400.0f*abs(1.0f - cosChange) / (1.0f + norm2);// a direction change is less of a problem, if the distance is larger
//				float sinChange = t2.cross(t2Prev);
//				e[3] += 400.0f*(1.0f-sqr(sinChange)) / (1.0f + norm2);// a direction change is less of a problem, if the distance is larger
			}

			if(n & 0x1)															// the odd indices are the paths of the last button of a combo
			{
				float cosPrev = dir ^ dirPrev;
				e[4] += 2.0f*abs(1.0f - cosPrev);

				cosPrev = hit ^ hitPrev;
				e[4] += 2.0f*abs(1.0f - cosPrev);
			}
			else
			{
				float sinPrev	= dir.cross(dirPrev);
				e[4] += 2.0f*abs(sinPrev);									// Penalize a 90° change most severely, 180° is good too, between combos
			}

			t2Prev = t2;

		}//if(n >= 1)

		dirPrev = dir;
		hitPrev = hit;

	}//for all paths

//----
//	A little complicated way to test for degenerate numbers, while allowing non IEEE754 math.
	const float f = (e[0]+e[1]+e[2]+e[3]+e[4]+e[5]+bias) / float(N);
	M128 testNaN;
	testNaN.f32[0] = f;
	testNaN.m128 = _mm_cmpunord_ss(testNaN.m128,testNaN.m128);

	if(testNaN.i32[0])
		fitness[j] = FLT_MAX;
	else
		fitness[j] = f;
}

/*
//-----------------------------------------------------------------------------
//	f(x) = sin(phi)
float arc_len_sin(float phi)
{
	return sqrt(2.0f)*boost::math::ellint_2<float>(sqrt(0.5f),phi);
}

//-----------------------------------------------------------------------------
//	f(x) = a * sin(b*x)
//	=> arc_length(x) = ∫ sqrt[(a*b*cos(b*x))^2+1] dx
float arc_len_sin(float x, float a, float b)
{
	float r = a*a*b*b,
			s = 1.0f / (r + 1.0f),
			t = cos(2.0f*b*x),
			u = r*t + r + 2.0f,
			v = sqrt(u),
			w = b * sqrt(u*s);

	return v / w * sqrt(2.0f) * boost::math::ellint_2<float>(sqrt(r*s),b*x);
}
*/

//-----------------------------------------------------------------------------
//	Creates and optimizes bezier paths, including end points.
//-----------------------------------------------------------------------------

void OptimizePaths(std::vector<FlyingButton> &buttons, int POP, float aspectRatio)
{
	Counter timer;
	unsigned num = buttons.size();
	TestingThreadSleep = 1;					// Alert testing thread
	StopOptimizing = false;

	POP = (POP+7) & ~7;						// make pop a multiple of 8

//----
//	Count combos, their lengths, start and end times, and temporal distances converted to spatial distances.
	std::vector<int>		comboCount;
	std::vector<float>	comboStartTime,
								comboEndTime,
								distances(1);
	for(unsigned i = 0; ; i++)
	{
		float endTime = i ? buttons[i-1].time : 0;

	//----
	//	Combine single buttons and small distance combos into unified combos, for better playability
		unsigned n = comboCount.size();
		if(n && i < num && buttons[i].newCombo)
		{
			float comboEnd;
		//----
		//	find combo end
			for(unsigned j = i+1; ; j++)
			{
				if(j==num || buttons[j].newCombo)
				{
					comboEnd = buttons[j-1].time;
					break;
				}
			}

			if((comboEnd-comboStartTime[n-1]) * TIME_TO_DISTANCE < 0.6f*W)
				buttons[i].newCombo = false;
		}

		if(i==0 || i==num || buttons[i].newCombo)
		{
			if(i!=num)
			{
				comboCount.push_back(0);
				comboStartTime.push_back(buttons[i].time);
			}

			if(i)
			{
				comboEndTime.push_back(endTime);
				if(i==num)
				{
					distances.push_back((endTime-comboStartTime[comboStartTime.size()-1]) * TIME_TO_DISTANCE);
					break;
				}
				else
					distances.push_back((endTime-comboStartTime[comboStartTime.size()-2]) * TIME_TO_DISTANCE);

				distances.push_back((comboStartTime[comboStartTime.size()-1]-endTime) * TIME_TO_DISTANCE);
			}
		}

		comboCount[comboCount.size()-1]++;
	}


//----
//	Create tables with each buttons combo index, its number in the combo and normalized positions by their relative time in the combo.
	std::vector<int>   combo	(num);
	std::vector<int>   comboPos(num);
	std::vector<float> comboPosNorm(num);
	int pos = 0,
			c = 0;
	for(unsigned i = 0; i < num; i++, pos++)
	{
		if(buttons[i].newCombo && i)
		{
			pos = 0;
			c++;
		}

		combo[i] = c;
		comboPos[i] = pos;
		if(comboCount[c] > 1)	comboPosNorm[i] = (buttons[i].time-comboStartTime[c]) / (comboEndTime[c]-comboStartTime[c]);
		else							comboPosNorm[i] = 0;		// only one button in the combo, which will use the first curve.
	}

//----
//	Each combo gets two cubic bezier curves, which are linearly interpolated between the combo members.
//	I.e., the first button of a combo flies on path[0], the last on path[1] and the others inbetween, depending on their relative time.
	const int DIM = 2*comboCount.size();					// problem dimension = number of paths = 2 for each combo

	Paths ga(DIM,POP);

	ga.generatorVariables[CoSyNE::MutateGamma]= 1.0f;
	ga.generatorVariables[CoSyNE::MutateAlpha]= 1.8f;
	ga.generatorVariables[CoSyNE::MutateProb]	= 0.5f;
	ga.generatorVariables[CoSyNE::RecombProb]	= 0.5f;
	ga.distances = &distances[0];

	ga.randomizePop();

	paths = &ga;

//----
//	Evolve a few generations
	Mem2D<int>			index(POP);
	const int			lambda	= POP/4;
	const int*			rank		= ga.getRanking();

	for(int generation = 0; ; generation++)
	{
	//----
	//	Tell the thread to test half the population
		if(ThreadState == ready)
			ThreadState = evaluate;

	//----
	//	Test our share of the samples
		for(int j = POP/2; j < POP; j++)
		{
			float fitnessBias = ga.arrangeDistance(j);

			ga.evaluate(j, fitnessBias);
		}

	//----
	//	wait for thread to finish
		while(ThreadState==evaluate)
			Sleep(1);

		ga.rankFitness();
		PTT::Quality = 500.0 / ga.getFitness()[rank[0]];

		if(StopOptimizing)
			break;

	//----
	//	create permutation for random selection of parents
		for(int j = 0; j < lambda; j++)
			index[j] = rank[j];

		ga.rng.shuffle(&index[0],lambda);

	//----
	//	recombination of every 2 parents into 2 children
		for(int j = 0; j < lambda; j+=2)
		{
			int src1 = index[j],
				 src2 = index[j+1],
				 dst1 = rank[POP-lambda+j],
				 dst2 = rank[POP-lambda+j+1];
			int rnd1 = rank[POP-1-lambda-j],
				 rnd2 = rank[POP-1-lambda-j-1];

			if(generation < 8)
			{
				ga.randomizeSample(rnd1);
				ga.randomizeSample(rnd2);
			}

			ga.samplesRecombine(src1,src2,dst1,dst2);

			ga.sampleMutateStrategyWeights(dst1);
			ga.sampleMutateStrategyWeights(dst2);
			ga.sampleMutateWeights(dst1);
			ga.sampleMutateWeights(dst2);
		}

	//----
	//	Permutation magic
		ga.samplesPermute(POP/2, 1.0, 0, DIM);


//		std::cout << "fitness of best sample: " << bestSample << std::endl;

//		UpdateCaption(wstringify(bestSample).c_str());
	}





//----
//	Put testing thread to sleep
	TestingThreadSleep = 20;
	paths = NULL;


//----
//	Write the best paths to the FlyingButton array
	int		best		= rank[0];
				c			= -1;													// current combo
	Path		*first	= NULL,												// first path of current combo
				*last		= NULL,												// last  path of current combo
				*paths	= ga.getPaths(best);								// paths of all combos
	float		omega1	= 0,													// angle of the first path
				delta		= 0;													// angle of the last path - angle of the first path

//----
//	Use a BSpline to map the bezier curves relative length, to the parameter t.
//	Must be fairly high resolution, especially at the end, since this is the most problematic, but also most important area.
	const int	subdiv = 4*1024,
					offset = subdiv/4;
	float f = 1.0f / (subdiv-(2*offset));
	Mem2D<float> tFunction(subdiv,2);

//----
//	initialize with identity function; which will be used in the range outside [0,1], to avoid boundary effects.
	for(int x = 0; x < subdiv; x++)
	{
		float t = f*(x-offset);
		tFunction(x,0) =
		tFunction(x,1) = t;
	}

//----
	for(unsigned i = 0; i < num; i++)
	{
	//----
	//	Are we quitting? Then exit right away.
		if(ThreadState == quit)
			return;

	//----
	//	interpolate the combo curves for this button
		if(c < combo[i])
		{
			first	= paths;
			last	= paths+1;
			paths += 2;
			c++;

			omega1 = first->dir.angle() + float(2.0*PI);
			delta  = last ->dir.angle() + float(2.0*PI) - omega1;
			while(delta < -PI)	delta += float(2.0*PI);
			while(delta >  PI)	delta -= float(2.0*PI);
		}

		float	f1 = 1.0f-comboPosNorm[i],
				f2 = comboPosNorm[i];

	//----
	//	save everything
		Path	&path = buttons[i].path;
		path.end		= first->end*f1    + last->end*f2;
		path.dir		= Point::FromAngle(omega1 + delta*f2);			// interpolate the angle
		path.alpha1	= first->alpha1*f1 + last->alpha1*f2;
		path.omega1	= first->omega1*f1 + last->omega1*f2;
		path.alpha2	= first->alpha2;
		path.omega2	= first->omega2;

	//----
		Point perp1	= path.dir.perp(),
				perp2	= perp1,
				start;

	//----
	//	get points and curve lengths for equally spaced t
		float	distance	= 0;
		Point	p, pPrev, pathPos;
		for(int x = offset; x <= 3*offset; x++)
		{
			float t = f*(x-offset),
					s_ = 1024.0f*(1.0f-t);	// parameter s = distance on the straight line

			p = path.end - path.dir * s_;

			float	phi1	= path.omega1 * s_,
					sin1	= path.alpha1 * sin(phi1);
			float	phi2	= path.omega2 * s_,
					cos2	= path.alpha2 * cos(phi2);

			if(path.omega2 > 0)	cos2 = cos2-path.alpha2;
			else						cos2 = path.alpha2-cos2;

			p += perp1*sin1;

			if(x != offset)
			{
				perp2 = (pathPos - p).perp().normalize();
				pathPos = p;
			}

			p += perp2*cos2;

		//----
			if(x == offset)
			{
				start = p;
				pathPos = p;
			}

		//----
		//	aspect ratio correction
			p.x *= aspectRatio*0.75f;

			if(x != offset)
			{
				distance += (p-pPrev).norm();
				tFunction(x,0) = distance;
				tFunction(x,1) = t;
			}

			pPrev = p;
		}

	//----
	//	normalize curve lengths
		float _1_norm = 1.0f / distance;
		for(int x = offset+1; x <= 3*offset; x++)
			tFunction(x,0) *= _1_norm;


//		float path_length = arc_len_sin(2.0f*W, buttons[i].path.alpha1, buttons[i].path.omega1);

		buttons[i].s.x = double(start.x);
		buttons[i].s.y = double(start.y);
		buttons[i].duration = double(distance) / (1.2*buttons[i].v.x);
		buttons[i].t0 = buttons[i].time - buttons[i].duration;
		buttons[i].tFunction = new BSpline<float>(&tFunction(0,0), tFunction.m(), &tFunction(0,1), 0.005);
		buttons[i].trail = start;
	}

}



